#include "search.h"
#if(KEIL)
#include "SEEKFREE_MT9V03X.h"
#endif


int IMAGE_MIDDLE = 130;
/********************************************变量声明***************************************************/
uint8        g_VideoImageData[MAX_VIDEO_LINE][MAX_VIDEO_POINT];  //图像原始数据，g_VideoImageData[m][n]代表图像的从上数第m行，从左数第n点，0为最黑，255为最白
//==============================图像数据================================//
_POINT  g_CenterPosition[MAX_POINT_NUM];
_POINT  g_LeftEdge[MAX_POINT_NUM], g_RightEdge[MAX_POINT_NUM];
int          g_CenterNum;
int          g_LeftEdgeNum, g_RightEdgeNum;
//==============================图像处理用变量==========================//
uint16       g_Start[10], g_End[10], g_SEnum;
uint8        g_SearchFlag = 1;
int          g_Cover;
uint8        g_CoverIndex[10], g_CoverNum;
//==============================控制用变量==============================//
int			g_DirectionControl = MID_VIDEO_POINT;
int			g_FormerDirectionControl = MID_VIDEO_POINT;
int			g_DirectionControlWhole = 0;
int			g_DirectionControlLine = 0;


//==============================新添的变量==============================//
float        g_LeftDown_x, g_LeftDown_y, g_LeftUp_x, g_LeftUp_y, g_RightDown_x, g_RightDown_y, g_RightUp_x, g_RightUp_y;//左下、左上、右下、右上拐点数据
int			 g_ImageLine, g_sum;
int          banma = 0;
//======================================================================//

/*自适应动态阈值范围，获得一个相对最佳的阈值*/
void IMAGE_MIDDLE_SET()
{
	// 大津法
	unsigned long Amount = 0;
	unsigned long PixelBack = 0;
	unsigned long PixelshortegralBack = 0;
	unsigned long Pixelshortegral = 0;
	signed long PixelshortegralFore = 0;
	signed long PixelFore = 0;
	float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差
	signed short MinValue = 0, MaxValue = 255;
	unsigned char HistoGram[256] = { 0 };
	// g_VideoImageData[MAX_VIDEO_LINE][MAX_VIDEO_POINT]
	for (int i = 0; i < MAX_VIDEO_LINE; i++)
		for (int j = 0; j < MAX_VIDEO_POINT; j++)
			HistoGram[g_VideoImageData[i][j]]++;
	for (; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++);
	for (; MaxValue > MinValue && HistoGram[MaxValue] == 0 && MaxValue != MinValue; MaxValue--);
	if (MaxValue == MinValue)
	{
		IMAGE_MIDDLE = MaxValue;
	}
	else
	{
		if ((MinValue + 1) == MaxValue)
		{
			IMAGE_MIDDLE = MinValue;
		}
		else
		{
			Pixelshortegral = 0;
			SigmaB = -1;
			for (int i = MinValue; i <= MaxValue; i++)
			{
				Amount += HistoGram[i];  // 像素点总数
				Pixelshortegral += HistoGram[i] * i;  // 灰度值总数
			}
			float tmp = Pixelshortegral;
			float average_pixel = tmp / Amount;
			for (int i = MinValue; i <= MaxValue; i++)
			{
				PixelBack = PixelBack + HistoGram[i];  // 前景像素点数
				PixelFore = Amount - PixelBack;  // 背景像素点数
				OmegaBack = ((float)PixelBack) / Amount;  // 前景像素百分比
				OmegaFore = ((float)PixelFore) / Amount;  // 背景像素百分比
				PixelshortegralBack += HistoGram[i] * i; // 前景灰度值
				PixelshortegralFore = Pixelshortegral - PixelshortegralBack;  // 背景灰度值
				MicroBack = ((float)PixelshortegralBack) / PixelBack;  // 前景灰度百分比
				MicroFore = ((float)PixelshortegralFore) / PixelFore;  // 背景灰度百分比
				Sigma = PixelBack * (MicroBack - average_pixel) * (MicroBack - average_pixel) + PixelFore * (MicroFore - average_pixel) * (MicroFore - average_pixel);  // 类间方差
				if (Sigma > SigmaB)
				{
					SigmaB = Sigma;
					IMAGE_MIDDLE = i;
				}
			}
		}
	}
}

/*********	search:处理数据即测试算法的函数 ********/
void Search()
{
	g_LeftEdgeNum = 0; g_RightEdgeNum = 0;
	g_SearchFlag = 1;

	int i, line, white_width = 0;

	//=======================图像识别=========================//
	//PS：此时我们的图像已经提取完毕 我们从底部开始，向上识别图像
	//图像识别数据初始化

	g_CenterNum = 0;
	g_LeftEdgeNum = 0;
	g_RightEdgeNum = 0;
	g_SearchFlag = 1;
	IMAGE_MIDDLE_SET();

	for (line = 0; line <= MAX_VIDEO_LINE - 1; line++)
	{
		//首先 求出本行所有白块的位置
		g_SEnum = 0;  //本行的白块计数器清零
		if (g_VideoImageData[line][MAX_VIDEO_POINT - 1] > IMAGE_MIDDLE)  //大于阈值为白 小于阈值为黑
			g_Start[g_SEnum] = 0;
		for (i = MAX_VIDEO_POINT - 1; i > 0; i--)
		{
			if (g_VideoImageData[line][i] > IMAGE_MIDDLE && g_VideoImageData[line][i - 1] <= IMAGE_MIDDLE)
			{
				g_End[g_SEnum] = MAX_VIDEO_POINT - i;
				g_SEnum++;
			}
			else if (g_VideoImageData[line][i] <= IMAGE_MIDDLE && g_VideoImageData[line][i - 1] > IMAGE_MIDDLE)
			{
				g_Start[g_SEnum] = MAX_VIDEO_POINT - i - 1;
			}
			else if (g_SEnum >= 10)
			{
				break;
			}
		}
		if (g_VideoImageData[line][0] > IMAGE_MIDDLE && g_SEnum < 10)
		{
			g_End[g_SEnum] = MAX_VIDEO_POINT - 1;
			g_SEnum++;
		}
		//把最下面的那一行单独拿出来处理 做为后续处理的基础
		if (g_SearchFlag)                    //最下一行处理标志
		{
			int temp_mark = 0;
			if (g_SEnum == 0)
			{
				continue;
			}
			white_width = g_End[0] - g_Start[0];
			for (i = 1; i < g_SEnum; i++)  //直接求最宽的白块
			{
				if (g_End[i] - g_Start[i] >= white_width)
				{
					white_width = g_End[i] - g_Start[i];
					temp_mark = i;
				}
			}
			if (white_width > WIDTH)
			{
				g_SearchFlag = 0;
				//认为第一行最宽的白块便是所需白块
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[temp_mark];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[temp_mark];
			}
		}
		else
		{
			if (g_SEnum == 0)
			{
				break;
			}
			for (i = 0, g_CoverNum = 0; i < g_SEnum; i++)
			{
				//覆盖关系，因为图像是一个联通区域 于是找与上一行已经找到白块相联通的白块
				g_Cover = MIN(g_End[i], g_RightEdge[g_RightEdgeNum - 1].y) - MAX(g_Start[i], g_LeftEdge[g_LeftEdgeNum - 1].y);
				if (g_Cover > -1)
				{
					g_CoverIndex[g_CoverNum] = i;
					g_CoverNum++;
				}
			}
			if (g_CoverNum == 1) //如果只有一个联通的 直接取出这个联通白块
			{
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[g_CoverIndex[0]];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[g_CoverIndex[0]];
			}
			else if (g_CoverNum == 0)  //没有联通的白块 说明图像已经处理完毕
			{
				break;                 //跳出整个图像处理的循环 进入求中心部分
			}
			else //有多个白块   需要取舍
			{
				//取舍规则：先求出本行的白块的中心，再寻找与上一行白块中心最相近的一个白块
				int temp_mark, temp_dis, temp_center, last_center;
				temp_mark = g_CoverIndex[0];
				temp_center = (g_Start[g_CoverIndex[0]] + g_End[g_CoverIndex[0]]) / 2;
				last_center = (g_LeftEdge[g_LeftEdgeNum - 1].y + g_RightEdge[g_RightEdgeNum - 1].y) / 2;
				temp_dis = ABS(last_center - temp_center);
				for (i = 1; i < g_CoverNum; i++)
				{
					temp_center = (g_Start[g_CoverIndex[i]] + g_End[g_CoverIndex[i]]) / 2;
					if (ABS(temp_center - last_center) < temp_dis)
					{
						temp_dis = ABS(temp_center - last_center);
						temp_mark = i;
					}
				}
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[temp_mark];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[temp_mark];
			}
		}
	}

	//=======================起点判断========================//
	//=======================十字处理========================//
	//=======================中心求取========================//
	/*
	*
	此处的中心求取非常简单粗糙，因为之前在边沿搜索的时候并没有区分两边，
	而是直接把一个白块的start认为是左边，end认为是右边 故中心数将与左右
	边的数相等，而中心位置直接由左右边相加除2所得
	*
	*/

	//g_CenterNum = g_RightEdgeNum;
	//for (i = 0; i < g_CenterNum; i++)
	//{
	//	g_CenterPosition[i].x = g_RightEdge[i].x;
	//	if (g_LeftEdge[i].y == 0 && g_RightEdge[i].y != 161) {
	//		g_CenterPosition[i].y = g_RightEdge[i].y - 80;
	//	}
	//	else if (g_LeftEdge[i].y != 0 && g_RightEdge[i].y == 161) {
	//		g_CenterPosition[i].y = g_LeftEdge[i].y + 80;
	//	}
	//	else {
	//		g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
	//	}
	//}


	int r_down_flag = 0;//右下拐点标志
	int r_up_flag = 0;//右上拐点标志
	int l_down_flag = 0;//左下拐点标志
	int l_up_flag = 0;//左上拐点标志
	int l_xie_crossroad_jin = 0;//近左斜标志
	int r_xie_crossroad_jin = 0;//近右斜标志
	int straightway = 0;//直道标志
	int before_crossroad = 0;//正十字前标志
	int ing_crossroad = 0;//正十字中标志
	int l_xie_crossroad_yuan = 0;//远左斜标志
	int r_xie_crossroad_yuan = 0;//远右斜标志
	int ing_crossroad_pd = 0;//正十字中与弯道离奇拐点、远斜十字判别标志
	for (i = 0; i < 20; i++)
	{
		if (g_VideoImageData[i][0] > IMAGE_MIDDLE && g_VideoImageData[i][161] > IMAGE_MIDDLE)
		{
			ing_crossroad_pd++;
		}
	}
	for (i = 5; i < 50; i++)//判断上面拐点类型
	{
		if (g_RightEdge[i].y - g_RightEdge[i + 1].y < 5 && g_RightEdge[i - 1].y - g_RightEdge[i].y > 5 && g_RightEdge[i - 2].y - g_RightEdge[i].y > 10 && g_LeftEdge[i - 10].y < 20)
		{
			r_up_flag = 1;
			g_RightUp_x = i;
			g_RightUp_y = g_RightEdge[i].y;
			//break;
			/*printf("%d\n",2);*/
		}
		if (g_LeftEdge[i + 1].y - g_LeftEdge[i].y < 5 && g_LeftEdge[i].y - g_LeftEdge[i - 1].y > 5 && g_LeftEdge[i].y - g_LeftEdge[i - 2].y > 10 && g_RightEdge[i - 10].y > 140)
		{
			l_up_flag = 1;
			g_LeftUp_x = i;
			g_LeftUp_y = g_LeftEdge[i].y;
			//break;
			/*printf("%d\n",4);*/
		}
	}
	for (i = 7; i < 40; i++)//判断下面拐点类型
	{
		if (g_RightEdge[i + 1].y - g_RightEdge[i].y > 5 && g_RightEdge[i + 2].y - g_RightEdge[i].y > 10 && g_RightEdge[i - 1].y - g_RightEdge[i].y < 5 && g_RightEdge[i - 4].y - g_RightEdge[i].y >= 0)
		{
			r_down_flag = 1;
			g_RightDown_x = i;
			g_RightDown_y = g_RightEdge[i].y;
			//break;
			/*printf("%d\n",1);*/
		}
		if (g_LeftEdge[i].y - g_LeftEdge[i + 1].y > 5 && g_LeftEdge[i].y - g_LeftEdge[i + 2].y > 10 && g_LeftEdge[i].y - g_LeftEdge[i - 1].y < 5 && g_LeftEdge[i].y - g_LeftEdge[i - 4].y >= 0)
		{
			l_down_flag = 1;
			g_LeftDown_x = i;
			g_LeftDown_y = g_LeftEdge[i].y;
			/*if (g_LeftDown_y > 81)
			{
				l_down_flag = 0;
			}*/
		}
	}
	if (r_down_flag == 1 && r_up_flag == 1 && l_down_flag == 0 && l_up_flag == 0)
	{
		l_xie_crossroad_jin = 1;
	}
	else if (r_down_flag == 0 && r_up_flag == 0 && l_down_flag == 1 && l_up_flag == 1)
	{
		r_xie_crossroad_jin = 1;
	}
	else if (r_down_flag == 1 && r_up_flag == 1 && l_down_flag == 1 && l_up_flag == 1)
	{
		before_crossroad = 1;
	}
	else if (r_down_flag == 0 && r_up_flag == 1 && l_down_flag == 0 && l_up_flag == 1 && ing_crossroad_pd > 10)
	{
		ing_crossroad = 1;
	}
	else
	{
		r_down_flag = 0, r_up_flag = 0, l_down_flag = 0, l_up_flag = 0;
	}


	if (r_down_flag == 0 && r_up_flag == 0 && l_down_flag == 0 && l_up_flag == 0 && l_xie_crossroad_jin == 0 && r_xie_crossroad_jin == 0)//远斜十字拐点判别
	{
		for (i = 0; i < 55; i++)
		{
			if (g_LeftEdge[i + 1].y == 0 && g_RightEdge[i - 1].y > g_RightEdge[i + 1].y && g_RightEdge[i + 2].y > g_RightEdge[i + 1].y && g_RightEdge[i + 1].y < 155 && g_RightEdge[i - 1].y < 155 && g_RightEdge[i + 2].y < 155)
			{
				r_down_flag = 1;
				g_RightDown_x = i + 1;
				g_RightDown_y = g_RightEdge[i + 1].y;
			}
			if (g_LeftEdge[i].y == 0 && g_LeftEdge[i + 1].y == 0 && g_LeftEdge[i + 2].y != 0 && g_LeftEdge[i + 3].y != 0 && g_LeftEdge[i + 2].y > 10)
			{
				r_up_flag = 1;
				g_RightUp_x = i + 2;
				g_RightUp_y = g_LeftEdge[i + 2].y;
			}
			if (g_RightEdge[i + 1].y == 161 && g_LeftEdge[i - 1].y < g_LeftEdge[i + 1].y && g_LeftEdge[i + 2].y < g_LeftEdge[i + 1].y && g_LeftEdge[i + 1].y > 5 && g_LeftEdge[i - 1].y > 5 && g_LeftEdge[i + 2].y > 5)
			{
				l_down_flag = 1;
				g_LeftDown_x = i + 1;
				g_LeftDown_y = g_LeftEdge[i + 1].y;
			}
			if (g_RightEdge[i].y == 161 && g_RightEdge[i + 1].y == 161 && g_RightEdge[i + 2].y != 161 && g_RightEdge[i + 3].y != 161 && g_RightEdge[i + 2].y < 151)
			{
				l_up_flag = 1;
				g_LeftUp_x = i + 2;
				g_LeftUp_y = g_RightEdge[i + 2].y;
			}
		}

	}

	int x_d = 0, x_u = 0;
	int r_turn_pd = 0;
	int l_turn_pd = 0;
	int r_near_turn = 0;//右近弯标志
	int r_far_turn = 0;//右远弯标志
	int l_near_turn = 0;//左近弯标志
	int l_far_turn = 0;//左远弯标志
	int r_turn_fuzhu = 0;
	int l_turn_fuzhu = 0;
	for (i = 0; i < 60; i++)
	{
		if (g_LeftEdge[i].y == 0 && g_RightEdge[i].y < 161 && g_RightEdge[i].y < g_RightEdge[i - 1].y)
		{
			l_turn_pd++;
		}
		if (g_RightEdge[i].y == 161 && g_LeftEdge[i].y > 0 && g_LeftEdge[i].y > g_LeftEdge[i - 1].y)
		{
			r_turn_pd++;
		}
		if (g_LeftEdge[i].y == 0)
		{
			l_turn_fuzhu++;
		}
		if (g_RightEdge[i].y == 161)
		{
			r_turn_fuzhu++;
		}
	}
	if (l_turn_pd > 30 && g_VideoImageData[l_turn_pd][100] < IMAGE_MIDDLE/*&& r_turn_fuzhu < 10*/)
	{
		l_near_turn = 1;
	}
	if (l_turn_pd > 10 && l_turn_pd < 30 && l_turn_fuzhu - l_turn_pd < 8)
	{
		l_far_turn = 1;
	}
	if (r_turn_pd > 30 && g_VideoImageData[r_turn_pd][60] < IMAGE_MIDDLE/*&& l_turn_fuzhu < 10*/)
	{
		r_near_turn = 1;
	}
	if (r_turn_pd > 10 && r_turn_pd < 30 && r_turn_fuzhu - r_turn_pd < 8)
	{
		r_far_turn = 1;
	}
	if (l_far_turn == 1 && r_far_turn == 1)
	{
		l_far_turn = 0;
		r_far_turn = 0;
	}


	if (l_far_turn == 1)
	{
		for (i = 20; i < 55; i++)
		{
			if (g_LeftEdge[i].y == 0 && g_LeftEdge[i - 1].y > 0)
			{
				x_d = i;
			}
			if (g_LeftEdge[i].y == 0 && g_LeftEdge[i + 1].y > 0)
			{
				x_u = i;
			}
		}
	}
	if (r_far_turn == 1)
	{
		for (i = 25; i < 55; i++)
		{
			if (g_RightEdge[i].y == 161 && g_RightEdge[i - 1].y < 161)
			{
				x_d = i;
			}
			if (g_RightEdge[i].y == 161 && g_RightEdge[i + 1].y < 161)
			{
				x_u = i;
			}
		}
		/*for (i = x_d; i < 55; i++)
		{
			if (g_RightEdge[i].y == 161 && g_RightEdge[i + 1].y < 161)
			{
				x_u = i;
			}
		}*/
	}
	if (x_d == 0 && x_u == 0)
	{
		l_far_turn = 0;
		r_far_turn = 0;
	}


	if (r_down_flag == 0 && r_up_flag == 0 && l_down_flag == 0 && l_up_flag == 0)
	{
		straightway = 1;
	}
	else if (r_down_flag == 1 && r_up_flag == 1 && l_down_flag == 0 && l_up_flag == 0 && l_xie_crossroad_jin == 0)
	{
		l_xie_crossroad_yuan = 1;
	}
	else if (r_down_flag == 0 && r_up_flag == 0 && l_down_flag == 1 && l_up_flag == 1 && r_xie_crossroad_jin == 0)
	{
		r_xie_crossroad_yuan = 1;
	}
	if (l_near_turn == 1 && l_xie_crossroad_jin == 1)
	{
		l_xie_crossroad_jin = 0;
	}
	if (r_near_turn == 1 && r_xie_crossroad_jin == 1)
	{
		r_xie_crossroad_jin = 0;
	}

	int l_ku = 0;
	int r_ku = 0;
	int l_ku_qian = 0;
	int l_ku_zhong = 0;
	int l_ku_hou = 0;
	int r_ku_qian = 0;
	int r_ku_zhong = 0;
	int r_ku_hou = 0;
	if (banma == 1)
	{
		l_down_flag = 0;
		l_up_flag = 0;
		r_down_flag = 0;
		r_up_flag = 0;
		int r_ku_pd = 0;
		int l_ku_pd = 0;
		for (i = 0; i < 60; i++)
		{
			if (g_LeftEdge[i].y == 0)
			{
				l_ku_pd++;
			}
			if (g_RightEdge[i].y == 161)
			{
				r_ku_pd++;
			}
		}
		if (l_ku_pd > r_ku_pd)
		{
			l_ku = 1;
		}
		else
		{
			r_ku = 1;
		}
		if (l_ku == 1)
		{
			for (i = 0; i < 30; i++)
			{
				if (g_LeftEdge[i - 1].y < g_LeftEdge[i + 1].y && g_LeftEdge[i + 2].y < g_LeftEdge[i + 1].y)
				{
					l_down_flag = 1;
					g_LeftDown_x = i + 1;
					g_LeftDown_y = g_LeftEdge[i + 1].y;
				}
			}
			for (i = 55; i > 35; i--)
			{
				if (g_LeftEdge[i + 1].y - g_LeftEdge[i].y < 5 && g_LeftEdge[i].y - g_LeftEdge[i - 1].y > 5)
				{
					l_up_flag = 1;
					g_LeftUp_x = i;
					g_LeftUp_y = g_LeftEdge[i].y;
				}
			}
			if (l_down_flag == 1 && l_up_flag == 1 && g_LeftDown_y < g_LeftUp_y)
			{
				l_ku_qian = 1;
			}
			if (l_ku_qian == 0 && l_up_flag == 1)
			{
				l_ku_zhong = 1;
			}
		}
		else if (r_ku == 1)
		{
			for (i = 0; i < 30; i++)
			{
				if (g_RightEdge[i - 1].y > g_RightEdge[i + 1].y && g_RightEdge[i + 2].y > g_RightEdge[i + 1].y)
				{
					r_down_flag = 1;
					g_RightDown_x = i + 1;
					g_RightDown_y = g_RightEdge[i + 1].y;
				}
			}
			for (i = 55; i > 38; i--)
			{
				if (g_RightEdge[i].y - g_RightEdge[i + 1].y < 3 && g_RightEdge[i - 1].y - g_RightEdge[i].y > 3)
				{
					r_up_flag = 1;
					g_RightUp_x = i;
					g_RightUp_y = g_RightEdge[i].y;
				}
			}
			if (r_down_flag == 1 && r_up_flag == 1 && g_RightUp_y < g_RightDown_y)
			{
				r_ku_qian = 1;
			}
			if (r_ku_qian == 0 && r_up_flag == 1)
			{
				r_ku_zhong = 1;
			}
		}
		r_near_turn = 0;
		l_near_turn = 0;
		r_far_turn = 0;
		l_far_turn = 0;
		r_xie_crossroad_yuan = 0;
		l_xie_crossroad_yuan = 0;
	}


	float b_r, k_r, b_l, k_l;//补线斜率和截距
	if (banma == 1)
	{
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = 81;
			}
	}
	else if (before_crossroad == 1)//正十字前中心求取
	{
		k_r = (g_RightUp_y - g_RightDown_y) / (g_RightUp_x - g_RightDown_x);
		b_r = g_RightDown_y - k_r * g_RightDown_x;
		for (i = g_RightDown_x; i < g_RightUp_x + 1; i++)
		{
			g_RightEdge[i].y = k_r * g_RightEdge[i].x + b_r;
		}
		k_l = (g_LeftUp_y - g_LeftDown_y) / (g_LeftUp_x - g_LeftDown_x);
		b_l = g_LeftDown_y - k_l * g_LeftDown_x;
		for (i = g_LeftDown_x; i < g_LeftUp_x + 1; i++)
		{
			g_LeftEdge[i].y = k_l * g_LeftEdge[i].x + b_l;
		}
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
	}
	else if (ing_crossroad == 1)//正十字中中心求取
	{
		k_r = (g_RightUp_y - 161) / g_RightUp_x;
		b_r = 161;
		for (i = 0; i < g_RightUp_x + 1; i++)
		{
			g_RightEdge[i].y = k_r * g_RightEdge[i].x + b_r;
		}
		k_l = g_LeftUp_y / g_LeftUp_x;
		b_l = 0;
		for (i = 0; i < g_LeftUp_x + 1; i++)
		{
			g_LeftEdge[i].y = k_l * g_LeftEdge[i].x;
		}
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
	}
	else if (l_xie_crossroad_jin == 1)//近左斜十字中心求取
	{
		k_r = (g_RightUp_y - g_RightDown_y) / (g_RightUp_x - g_RightDown_x);
		b_r = g_RightDown_y - k_r * g_RightDown_x;
		for (i = g_RightDown_x; i < g_RightUp_x + 1; i++)
		{
			g_RightEdge[i].y = k_r * g_RightEdge[i].x + b_r;
		}
		int left_create = g_RightUp_x;
		k_l = -k_r;
		b_l = g_LeftEdge[left_create].y - k_l * g_RightUp_x;
		for (i = left_create; i >= 0; i--)
		{
			g_LeftEdge[i].y = k_l * g_LeftEdge[i].x + b_l;
		}
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
	}
	else if (r_xie_crossroad_jin == 1)//近右斜十字中心求取
	{
		k_l = (g_LeftUp_y - g_LeftDown_y) / (g_LeftUp_x - g_LeftDown_x);
		b_l = g_LeftDown_y - k_l * g_LeftDown_x;
		for (i = g_LeftDown_x; i < g_LeftUp_x + 1; i++)
		{
			g_LeftEdge[i].y = k_l * g_LeftEdge[i].x + b_l;
		}
		int right_create = g_LeftUp_x;
		k_r = -k_l;
		b_r = g_RightEdge[right_create].y - k_r * g_LeftUp_x;
		for (i = right_create; i >= 0; i--)
		{
			g_RightEdge[i].y = k_r * g_RightEdge[i].x + b_r;
		}
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
	}
	else if (l_xie_crossroad_yuan == 1)//远左斜十字中心求取
	{
		k_r = (g_RightUp_y - g_RightDown_y) / (g_RightUp_x - g_RightDown_x);
		b_r = g_RightDown_y - k_r * g_RightDown_x;
		for (i = g_RightDown_x; i < g_RightUp_x + 1; i++)
		{
			g_RightEdge[i].y = k_r * g_RightEdge[i].x + b_r;
		}
		g_RightEdgeNum = g_RightUp_x;
		g_LeftEdgeNum = g_RightUp_x;
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = k_r * g_RightEdge[i].x + b_r / 2;
			}
	}
	else if (r_xie_crossroad_yuan == 1)//远右斜十字中心求取
	{
		k_l = (g_LeftUp_y - g_LeftDown_y) / (g_LeftUp_x - g_LeftDown_x);
		b_l = g_LeftDown_y - k_l * g_LeftDown_x;
		for (i = g_LeftDown_x; i < g_LeftUp_x + 1; i++)
		{
			g_LeftEdge[i].y = k_l * g_LeftEdge[i].x + b_l;
		}
		g_RightEdgeNum = g_LeftUp_x;
		g_LeftEdgeNum = g_LeftUp_x;
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_LeftEdge[i].x;
				g_CenterPosition[i].y = k_l * g_LeftEdge[i].x + (161 - b_l) / 2;
			}
	}
	else if (l_near_turn == 1)//左近弯中心求取
	{
		int left_pinyi = g_RightEdge[0].y;
		for (i = 0; i <= l_turn_pd; i++)
		{
			g_LeftEdge[i].y = g_RightEdge[i].y - left_pinyi - 2 * i;
		}
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
	}
	else if (r_near_turn == 1)//右近弯中心求取
	{
		int right_pinyi = 161 - g_LeftEdge[0].y;
		for (i = 0; i <= r_turn_pd; i++)
		{
			g_RightEdge[i].y = g_LeftEdge[i].y + right_pinyi + 2 * i;
		}
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
	}
	else if (l_far_turn == 1)
	{
		g_CenterNum = g_RightEdgeNum;
		g_LeftEdgeNum = x_d;
			for (i = 0; i <= x_d; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
		k_l = -2 * g_CenterPosition[x_d].y / (x_u - x_d);
		b_l = -k_l * (x_d + x_u) / 2;
		for (i = x_d; i <= g_RightEdgeNum/*(x_d + x_u) / 2*/; i++)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y = k_l * i + b_l;
		}
	}
	else if (r_far_turn == 1)
	{
		g_CenterNum = g_LeftEdgeNum;
		g_RightEdgeNum = x_d;
			for (i = 0; i <= x_d; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
		k_r = 2 * (161 - g_CenterPosition[x_d].y) / (x_u - x_d);
		b_r = 161 - k_r * (x_d + x_u) / 2;
		for (i = x_d; i <= g_RightEdgeNum/*(x_d + x_u) / 2*/; i++)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y = k_r * i + b_r;
		}
	}
	else if (l_ku_qian == 1)
	{
		g_CenterNum = g_LeftUp_x;
		g_RightEdgeNum = g_LeftUp_x;
		g_LeftEdgeNum = g_LeftDown_x;
		int start = g_LeftDown_x;
		int end_x = (g_LeftDown_x + g_LeftUp_x) / 2;
		int end_y = (g_LeftDown_y + g_LeftUp_y) / 2;
		k_l = (g_RightEdge[start].y - g_LeftUp_y) / (start - g_LeftUp_x);
		b_l = g_LeftUp_y - k_l * g_LeftUp_x;
		for (i = start; i < g_LeftUp_x+1; i++)
		{
			g_RightEdge[i].y = k_l * i + b_l;
		}
		for (i = 0; i < start; i++)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
		}
		for (i = start; i < g_LeftUp_x; i++)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y = (k_l-1) * i + b_l + 81 - g_RightEdge[start].y;
		}
		for (i = end_x; i < g_LeftUp_x; i++)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y =  -i + b_l + 81 - g_RightEdge[start].y;
		}
    }
	else if (r_ku_qian == 1)
	{
		g_CenterNum = g_RightUp_x;
		g_RightEdgeNum = g_RightDown_x;
		g_LeftEdgeNum = g_RightUp_x;
		int start = g_RightDown_x;
		int end_x = (g_LeftDown_x + g_LeftUp_x) / 2;
		int end_y = (g_LeftDown_y + g_LeftUp_y) / 2;
		k_r = (g_LeftEdge[start].y - g_RightUp_y) / (start - g_RightUp_x);
		b_r = g_RightUp_y - k_r * g_RightUp_x;
		for (i = start; i < g_RightUp_x + 1; i++)
		{
			g_LeftEdge[i].y = k_r * i + b_r;
		}
		for (i = 0; i < start; i++)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
		}
		for (i = start; i < g_RightUp_x; i++)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y = (k_r + 1) * i + b_r + 81 - g_LeftEdge[start].y;
		}
	}
	else if (l_ku_zhong == 1)
	{
		g_CenterNum = g_LeftUp_x;
		g_RightEdgeNum = g_LeftUp_x;
		g_LeftEdgeNum = g_LeftUp_x;
		k_l = (161 - g_LeftUp_y) / (-g_LeftUp_x);
		b_l = 161;
		for (i = 0; i < g_LeftUp_x; i++)
		{
			g_RightEdge[i].y = k_l * i + b_l;
			g_LeftEdge[i].y = k_l * i;
		}
		for (i = 0; i < g_LeftUp_x; i++)
		{
			g_CenterPosition[i].x = g_RightEdge[i].x;
			g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
		}
	}
	else if (r_ku_zhong == 1)
	{
		g_CenterNum = g_RightUp_x;
		g_RightEdgeNum = g_RightUp_x;
		g_LeftEdgeNum = g_RightUp_x;
			k_r = g_RightUp_y / g_RightUp_x;
		for (i = 0; i < g_RightUp_x; i++)
		{
			g_LeftEdge[i].y = k_r * i;
			g_RightEdge[i].y = k_r * i + 161;
		}
		for (i = 0; i < g_RightUp_x; i++)
		{
			g_CenterPosition[i].x = g_RightEdge[i].x;
			g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
		}
	}
	else
	{
		g_CenterNum = g_RightEdgeNum;
			for (i = 0; i < g_RightEdgeNum; i++)
			{
				g_CenterPosition[i].x = g_RightEdge[i].x;
				g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
			}
	}

	//===================控制中心及电机控制====================//
	//加权平均法求方向控制
	g_DirectionControlWhole = 0;
	g_DirectionControlLine = 0;
	for (i = 0; i < g_CenterNum; i++)
	{
		if (g_CenterPosition[i].y >= 0 && g_CenterPosition[i].y <= MAX_VIDEO_POINT)
		{
			g_DirectionControlLine += (int)g_CenterPosition[i].x;
			g_DirectionControlWhole += (int)g_CenterPosition[i].y * g_CenterPosition[i].x;  //注意数据不要溢出
		}
	}
	if (g_DirectionControlLine > 0)
	{
		g_DirectionControl = g_DirectionControlWhole / g_DirectionControlLine;
	}
	//方向控制限定
	if (g_DirectionControl < 0)
	{
		g_DirectionControl = 0;
	}
	else if (g_DirectionControl > MAX_VIDEO_POINT)
	{
		g_DirectionControl = MAX_VIDEO_POINT;
	}
	if (ABS(g_DirectionControl - g_FormerDirectionControl) > 90)
	{
		g_DirectionControl = g_FormerDirectionControl;
	}
}
